#include "DTWX3Data.h"

#include <stdlib.h>
#include <string>
#include <sstream>
#include <iomanip>
#include <string.h>
#include <stdio.h>

// Floating point precision
#define SENTENCE_FP_PRECISION 2

// Left motor PWM
#define MIN_LEFT_MOTOR 500
#define MAX_LEFT_MOTOR 2500
#define DEFAULT_LEFT_MOTOR MIN_LEFT_MOTOR
// Right motor PWM
#define MIN_RIGHT_MOTOR 500
#define MAX_RIGHT_MOTOR 2500
#define DEFAULT_RIGHT_MOTOR MIN_RIGHT_MOTOR
// Rear motor power (%)
#define MIN_REAR_MOTOR 0
#define MAX_REAR_MOTOR 100
#define DEFAULT_REAR_MOTOR MIN_REAR_MOTOR
// Left servo angle (deg)
#define MIN_LEFT_SERVO -45
#define MAX_LEFT_SERVO 45
#define DEFAULT_LEFT_SERVO 0
// Right servo angle (deg)
#define MIN_RIGHT_SERVO -45
#define MAX_RIGHT_SERVO 45
#define DEFAULT_RIGHT_SERVO 0
// Rear servo angle (deg)
#define MIN_REAR_SERVO -30
#define MAX_REAR_SERVO 30
#define DEFAULT_REAR_SERVO 0

/*
 * Constructor
 */
DTWX3Data::DTWX3Data() 
    : m_left_motor(DEFAULT_LEFT_MOTOR), 
    m_right_motor(DEFAULT_RIGHT_MOTOR), 
    m_rear_motor(DEFAULT_REAR_MOTOR),
    m_rear_servo(DEFAULT_REAR_SERVO),
    m_numGeneratedSentences(0),
    m_numParsedSentences(0)
{}
/*
 * Setters return 
 * 0 on succesful set, 
 * -666 if outside lower boundary
 * 666 if outside upper boundary
*/
int DTWX3Data::SetLeftMotor(const int& value) 
{
    if (value < MIN_LEFT_MOTOR)
        return -666;
    else if (value > MAX_LEFT_MOTOR)
        return 666;
    
    m_left_motor = value;
    
    return 0;
}
int DTWX3Data::SetRightMotor(const int& value) 
{
    if (value < MIN_RIGHT_MOTOR)
        return -666;
    else if (value > MAX_RIGHT_MOTOR)
        return 666;
    
    m_right_motor = value;
    
    return 0;
}
int DTWX3Data::SetRearMotor(const float& value)
{
    if (value < MIN_REAR_MOTOR)
        return -666;
    else if (value > MAX_REAR_MOTOR)
        return 666;
    
    m_rear_motor = value;
    
    return 0;
}
int DTWX3Data::SetRearServo(const float& value) 
    {
    if (value < MIN_REAR_SERVO)
        return -666;
    else if (value > MAX_REAR_SERVO)
        return 666;

    m_rear_servo = value;

    return 0;
}
/*
DTWX3 Sentence format

$DTWX3,50,50,15,10,15,50*6A
	|   |  |  |  |  |  |  |
	|   |  |  |  |  |  |  6A Checksum data
	|   |  |  |  |  |  |
	|   |  |  |  |  |  rear servo angle (degrees)
	|   |  |  |  |  |
	|   |  |  |  |  right servo angle (degrees)
	|   |  |  |  |
	|   |  |  | left servo angle (degrees)
	|   |  |  |
	|   |  | rear motor power percentage 
    |   |  |    
	|   |  right motor PWM
	|   |
	|   left motor PWM
	|
	DTWX3 Desktop Works Sentence format for X3 vessel
	*/
int DTWX3Data::ProcessDTWX3(char *sentence)
{
	char *token = NULL;

    token = strtok(sentence, ",");
    if (strcmp(token, "$DTWX3"))
        return -666; // Invalid sentence

	// left motor
	token = strtok(NULL, ",");
    SetLeftMotor(atoi(token));

    // right motor
	token = strtok(NULL, ",");
	SetRightMotor(atoi(token));
	
	// rear motor
	token = strtok(NULL, ",");
	SetRearMotor((float)atof(token));

	// rear servo
	token = strtok(NULL, ",");
    SetRearServo((float)atof(token));
    		
	// another sentence parsed
	++m_numParsedSentences;

    return 0;
}

int DTWX3Data::GenerateDTWX3(char* sentence)
{
	std::stringstream stream;

	// Send data to stream
	stream << "DTWX3,";
    stream << GetLeftMotor() << ",";
    stream << GetRightMotor() << ",";
    stream << std::setiosflags(std::ios::fixed) << std::setprecision(SENTENCE_FP_PRECISION) << GetRearMotor() << ",";
	stream << std::setiosflags(std::ios::fixed) << std::setprecision(SENTENCE_FP_PRECISION) << GetRearServo();

	// XOR checksum
	unsigned calcChecksum = 0;
	for (size_t i = 0; i < stream.str().size(); ++i)
		calcChecksum ^= stream.str()[i];
	
	// Checksum to hex to string
	char checkBuffer[5];
	sprintf(checkBuffer, "%02lX\r\n", calcChecksum);
	
	// Finalize the sentence
    strcpy(sentence, std::string("$" + stream.str() + "*" + std::string(checkBuffer)).c_str());

    // another sentence generated
	++m_numGeneratedSentences;
	
	return 0;
}
